#include "app.h"
#include "Logout.h"
#include "ip2366.h"
#include "mavlink_interface.h"
#include "tim.h"
#include "adc.h"
#include "pid.h"
#include "ws2812b.h"
#include "stdbool.h"

static const char *TAG = "app";
ip2366_state_t ip = {0};
mavlink_interface_t mav = {0};
system_state_t sys = {0};
ws2812 ws;
extern DMA_HandleTypeDef hdma_tim15_ch1;

void APPInit(void) {
    // // ip2366初始化
    // ip.ip2366i2c = &hi2c1;
    // ip.IP_INT_GPIOx = GPIOB;
    // ip.IP_INT_GPIO_Pin = GPIO_PIN_9;
    // ip.type = 1;                // LiPo电池
    // ip.battery_Vfull = 8400;   // mV
    // ip.battery_Vempty = 7400;  // mV
    // ip.battery_capacity = 3000; // mAh
    //

    // if (!DEBUG) IP2366_Init(&ip);

    // mavlink初始化
    mav.mavuart = &huart2;
    mav.my_sysid = 1;
    mav.my_compid = 1;
    HAL_UART_Receive_IT(mav.mavuart, mav.uart_rx_buf, 1);
    __HAL_UART_ENABLE_IT(mav.mavuart, UART_IT_ERR);

    // sys默认参数及pid初始化
    sys.sys_state = SYS_OFF;
    sys.sys_state_next = SYS_ON;
    sys.cap_current_target = 0.2f;
    sys.lpf = 0.9999f; // 0.999f
    HAL_ADCEx_Calibration_Start(&hadc1, ADC_SINGLE_ENDED);
    HAL_ADCEx_Calibration_Start(&hadc2, ADC_SINGLE_ENDED);
    HAL_ADC_Start_DMA(&hadc1, (uint32_t *) sys.adc1_buf, 1);
    HAL_ADC_Start_DMA(&hadc2, (uint32_t *) sys.adc2_buf, 1);
    HAL_TIM_Base_Start_IT(&htim17);
    HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim16, TIM_CHANNEL_1);
    PID_init(&sys.current_loop, PID_DELTA, 1.0f * 20, 0.01f * 20, 0.11f * 20, 250.0f, 0.0f, 0.0f, 0.0f);
    PID_init(&sys.out_loop, PID_DELTA, 0.2f * 20, 0.03f * 20, 0.11f * 20, 250.0f, 0.0f, 0.0f, 0.0f);

    // ws2812初始化
    ws.config.htim = &htim15;
    ws.config.Channel = TIM_CHANNEL_1;
    ws.config.max_len = 2;
    ws2812_create(&ws);
    HAL_DMA_DeInit(&hdma_tim15_ch1);
    HAL_DMA_Init(&hdma_tim15_ch1);
    HAL_TIM_PWM_Stop_DMA(ws.config.htim, ws.config.Channel);
    color_rgb colors[2] = {0}; // 初始化关灯
    ws2812_set_array(&ws, colors, ws.config.max_len);

    // 按键Handle初始化
    HAL_TIM_Base_Start_IT(&htim7);
}

// uint32_t charge_cnt = 0;
static float voltage_variability = 0.0f;
static uint8_t boot_flag = 0;

void APPTask(void) {
    static uint32_t shutdown_timer = 0;
    static uint32_t powerup_timer = 0;
    static bool shutdown_condition_met = false;
    static bool powerup_condition_met = false;

    /* 不要电池 */
    // // 有电池电压低关系统（非常紧急，立即触发）
    // IP2366_GetState(&ip);
    // if (ip.mode == 2 && ip.voltages[0] <= BAT_VLOTAGE_THRESHOLD)
    // {
    //     sys.sys_state = 0;
    // }
    voltage_variability = 0.99f * voltage_variability + 0.01f * (sys.cap_vlotage - sys.cap_vlotage_last);

    if (voltage_variability > CAP_VLOTAGE_CHARGE_VARIBILITY) {
        sys.charge_state = CAP_CHARGE;
    } else if (voltage_variability < -CAP_VLOTAGE_DISCHARGE_VARIBILITY) {
        sys.charge_state = CAP_DISCHARGE;
    } else {
        sys.charge_state = CAP_STABLE;
    }

    if (sys.sys_state == sys.sys_state_next) {
        if ((sys.charge_state == CAP_CHARGE) && (sys.charge_state_last != sys.charge_state)) {
            // charge_cnt++;
            if (sys.sys_state == SYS_OFF) {
                boot_flag = true;
                powerup_condition_met = true;
                powerup_timer = HAL_GetTick();
            }
        }
    } else {
        boot_flag = false;
        powerup_condition_met = false;
    }

    sys.charge_state_last = sys.charge_state;
    sys.cap_vlotage_last = sys.cap_vlotage;

    if (sys.cap_vlotage > CAP_VLOTAGE_RAISE_THRESHOLD) {
        if (sys.sys_state == sys.sys_state_next) {
            if (sys.sys_state == SYS_ON) {
                if ((sys.cap_vlotage < CAP_VLOTAGE_DROP_THRESHOLD) && (sys.charge_state == CAP_DISCHARGE)) {
                    if (!shutdown_condition_met) {
                        shutdown_condition_met = true;
                        shutdown_timer = HAL_GetTick();
                    } else if (HAL_GetTick() - shutdown_timer >= BOOT_SHUTDOWN_DELAY_TIME) {
                        // 2 seconds delay
                        sys.sys_state_next = SYS_OFF;
                        shutdown_condition_met = false;
                    }
                } else {
                    shutdown_condition_met = false;
                }
            } else if (sys.sys_state == SYS_OFF) {
                if (boot_flag) {
                    if (powerup_condition_met && (HAL_GetTick() - powerup_timer >= BOOT_STARTUP_DELAY_TIME)) {
                        // 2 seconds delay
                        sys.sys_state_next = SYS_ON;
                        boot_flag = false;
                        powerup_condition_met = false;
                    }
                } else {
                    powerup_condition_met = false;
                }
            }
        }
    } else {
        if (!shutdown_condition_met) {
            shutdown_condition_met = true;
            shutdown_timer = HAL_GetTick();
        } else if (HAL_GetTick() - shutdown_timer >= BOOT_SHUTDOWN_DELAY_TIME) {
            // 2 seconds delay
            sys.sys_state_next = SYS_OFF;
            shutdown_condition_met = false;
        }
    }


    // mavlink报文处理
    if (mav.host_info_flag) {
        if (sys.sys_state_next == SYS_ON) {
            // // 3588标识位关闭电源树，等待10s后执行，重新发送刷新时间
            // if (!mav.host_info.IP_POWER_EN)
            // {
            //     sys.ip_delay = HAL_GetTick();
            // }
            // else if (mav.host_info.IP_POWER_EN == 1)
            // {
            //     sys.ip_state = 1;
            // }

            if (mav.host_info.PC_POWER_EN == 0) {
                sys.sys_state_next = SYS_OFF;
            }
            // else if (mav.host_info.PC_POWER_EN==1) {
            //     // 没有这个可能
            // }

            if (mav.host_info.PD_POWER_EN == 0) {
                sys.pd_state = 0;
            } else if (mav.host_info.PD_POWER_EN == 1) {
                sys.pd_state = 1;
            }

            // RGB更新
            if (sys.sys_state == sys.sys_state_next) {
                color_rgb colors[2];
                for (int i = 0; i < 2; i++) {
                    colors[i].r = mav.host_info.RGBRGB[i * 3];
                    colors[i].g = mav.host_info.RGBRGB[i * 3 + 1];
                    colors[i].b = mav.host_info.RGBRGB[i * 3 + 2];
                }
                ws2812_set_array(&ws, colors, ws.config.max_len);
            }
        }
        // 反馈报文发送
        // if (sys.sys_state) IP2366_GetState(&ip); // 主循环采集了
        mav.fdb_info.debug_key_state = HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_0) ? 1 : 0;
        if (sendFeedBackstatus(&mav, &ip, &sys)) {
            mav.host_info_flag = 0;
        }
    }

    if (sys.sys_state != sys.sys_state_next) {
        // 首次变更处理
        if (sys.sys_state_next_last != sys.sys_state_next) {
            if (sys.sys_state_next == SYS_OFF) {
                color_rgb colors[2] = {138, 54, 15, 138, 54, 15}; // 关机红灯
                ws2812_set_array(&ws, colors, ws.config.max_len);
                sys.pc_delay = HAL_GetTick(); // 等待关机
            } else if (sys.sys_state_next == SYS_ON) {
                color_rgb colors[2] = {0, 255, 0, 0, 255, 0}; // 开机绿灯
                ws2812_set_array(&ws, colors, ws.config.max_len);
            }
        }
        sys.sys_state_next_last = sys.sys_state_next;

        // 执行开机或者关机
        if (sys.sys_state == SYS_ON) {
            if (HAL_GetTick() - sys.pc_delay > SHUT_DOWN_THRESHOLD) {
                sys.pc_state = false;
                sys.pd_state = false;

                HAL_GPIO_WritePin(GPIOB, GPIO_PIN_6, GPIO_PIN_RESET);
                HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, GPIO_PIN_RESET);

                color_rgb colors[2] = {0}; // 关机关灯
                ws2812_set_array(&ws, colors, ws.config.max_len);

                sys.sys_state = SYS_OFF;
                HAL_Delay(500); //阻塞掉电压回升造成的误触发，不影响中断长按
            }
        } else if (sys.sys_state == SYS_OFF) {
            sys.pc_state = true;
            sys.sys_state = SYS_ON;
        }
    }

    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, sys.pd_state ? GPIO_PIN_SET : GPIO_PIN_RESET);
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_6, sys.pc_state ? GPIO_PIN_SET : GPIO_PIN_RESET);
    HAL_Delay(1);
}

// 长按开关PC和IP
void LONG_PRESS_Handle(void) {
    if (sys.sys_state == sys.sys_state_next) {
        if (sys.sys_state == SYS_ON) {
            sys.sys_state_next = SYS_OFF;
        } else if ((sys.sys_state == SYS_OFF) && sys.cap_vlotage > CAP_VLOTAGE_DROP_THRESHOLD) {
            sys.sys_state_next = SYS_ON;
        }
    }
}

// 按钮开关PD电源
void SHORT_PRESS_Handle(void) {
    if (sys.pd_state) {
        // HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, GPIO_PIN_RESET);
        sys.pd_state = 0;
    } else {
        // HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, GPIO_PIN_SET);
        sys.pd_state = 1;
    }
}

void TIM7_Handle(void) {
    // uart_dma_printf(&huart2, "%d,%d,%d\n", (uint32_t)(sys.cap_current*1e3),(uint32_t)sys.ratio,(uint32_t)(sys.cap_vlotage*1e1));
    static uint32_t press_start_time = 0;
    static uint8_t is_pressed = 0;
    static uint8_t long_press_handled = 0; // 新增标志位
    static uint8_t press_count = 0; // 按键计数
    static uint32_t last_press_time = 0; // 上一次按键时间

    sys.sw_state = HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_13);

    if (sys.sw_state == 0) {
        // 按键按下
        if (!is_pressed) {
            is_pressed = 1;
            press_start_time = 0; // 重新计时
            long_press_handled = 0; // 重置长按处理标志位
            last_press_time = 0;
        } else {
            press_start_time++;
        }

        // 长按时间超过阈值触发
        if (press_start_time >= LONG_PRESS_THRESHOLD && !long_press_handled) {
            LONG_PRESS_Handle();
            long_press_handled = 1; // 设置标志位，确保只执行一次
        }
    } else {
        // 按键释放
        // 短按时间超过阈值并松手触发
        if (is_pressed) {
            if (press_start_time >= SHORT_PRESS_THRESHOLD) {
                press_count++;
            }
            is_pressed = 0;
        }
        last_press_time++;
        if (last_press_time > TRIPLE_PRESS_THRESHOLD) {
            if (press_count && !long_press_handled) {
                // if (press_count == 1) {
                //     SHORT_PRESS_Handle();
                // }
                mav.fdb_info.power_key_state[0] = press_count;
                mav.fdb_info.power_key_state[1]++;
                press_count = 0;
            }
            last_press_time = 0;
        }
    }
}


// pid电容充放电控制
// 20KHz
void TIM17_Handle(void) {
    sys.cap_vlotage = sys.cap_vlotage * sys.lpf + (1 - sys.lpf) * (float) (uint16_t) sys.adc1_buf[0] * V_RATIO;
    sys.cap_current = sys.cap_current * sys.lpf +
                      (1 - sys.lpf) * (float) ((uint16_t) sys.adc2_buf[0] - I_BIAS) * I_RATIO_A1_10M;
    if (sys.cap_current_target > 0) {
        sys.ratio = PID_calc(&sys.current_loop, sys.cap_current, sys.cap_current_target);
        TIM16->CCR1 = (uint32_t) (sys.ratio);
        TIM4->CCR1 = 0;
    } else {
        if (sys.cap_vlotage > CAP_VLOTAGE_THRESHOLD) {
            sys.ratio = PID_calc(&sys.out_loop, -sys.cap_current, -sys.cap_current_target);
            TIM16->CCR1 = 1000;
            TIM4->CCR1 = (uint32_t) (sys.ratio);
        } else {
            TIM16->CCR1 = 1000;
            TIM4->CCR1 = 0;
        }
    }
}

void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim) {
    if (htim == ws.config.htim) {
        HAL_TIM_PWM_Stop_DMA(ws.config.htim, ws.config.Channel);
    }
}

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
    if (htim->Instance == TIM7) {
        TIM7_Handle();
    }
    if (htim->Instance == TIM17) {
        TIM17_Handle();
    }
}

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {
    if (huart->Instance == USART2) {
        MavlinkData(&mav, mav.uart_rx_buf[0]);
        __HAL_UART_CLEAR_PEFLAG(mav.mavuart);
        HAL_UART_Receive_IT(mav.mavuart, mav.uart_rx_buf, 1);
    }
}

void handleMessage(mavlink_interface_t *mav, fmav_message_t *msg) {
    switch (msg->msgid) {
        case FASTMAVLINK_MSG_ID_PL_HOST_INFO:
            fmav_msg_pl_host_info_decode(&mav->host_info, msg);
            mav->host_info_flag = 1;
            break;
    }
}
